
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <sys/types.h>
#include <pthread.h>
#include <sys/sysinfo.h>
#include <errno.h>
#include <signal.h>
#include <fcntl.h>
#include <ctype.h>
#include "mt03x.h"
#include "rs232.h"
//#include "ColorMap.h"

static const int m_temp_device = 8;
static const int m_big_sensor_show = 0;
static volatile float m_sensor_mt03x_temp = 0.0;
volatile float m_sensor_mt03x_ta = 0.0;
static int m_sensor_mt03x_size_width = 32;
static int m_sensor_mt03x_size_height = 32;
int m_sensor_mt03x_size_type = 1024;
volatile uint8_t m_sensor_mt03x_flush_ui_flag = 0;
volatile uint16_t m_sensor_mt03x_pos_x = 15;
volatile uint16_t m_sensor_mt03x_pos_y = 15;
int m_sensor_mt03x_type = 0;
uint16_t m_sensor_mt03x_otype = 0;

uint16_t mt03x_version = 0;
uint8_t mt03x_LMode = 0;
uint8_t mt03x_RMode = 0;
uint8_t mt03x_iir = 0;
uint8_t mt03x_mavg = 0;
uint16_t mt03x_epsilon = 0;
uint8_t mt03x_id_arr[4] = {0, 0, 0, 0};
uint32_t mt03x_id = 0;

const u8 sensor_mt03x_CONFIG_INIT_IIR_CMD[INIT_CMD_IIR_LENGTH] = "SetP=1,50\r\n";
const u8 sensor_mt03x_CONFIG_INIT_MAVG_CMD[INIT_CMD_MAVG_LENGTH] = "SetP=2,1\r\n";
const u8 sensor_mt03x_CONFIG_INIT_LMODE_CMD[INIT_CMD_LMODE_LENGTH] = "SetP=L,3\r\n";

#define HOTMAP_MAX (100)
static volatile uint8_t hotIndex = 0;
static uint8_t HOTMAP_NUM = 20;
static volatile float hotMap[HOTMAP_MAX];
static volatile float hotCalMap[HOTMAP_MAX];
void search_max_face_add(float face_temp);
float search_max_face_temp(void);

float avgTemp = 0.f;

void sensor_mt03x_init(void)
{
    m_sensor_mt03x_temp = 0.0;
    m_sensor_mt03x_size_type = 1024;
    mt03x_version = 0;
    mt03x_LMode = 0;
    mt03x_RMode = 0;
    mt03x_iir = 0;
    mt03x_mavg = 0;
    mt03x_epsilon = 0;
    avgTemp = 0.f;
}

const u8 sensor_mt03x_CONFIG_CMD[9] = "ConfigT\r\n";
u8 *sensor_mt03x_start(uint32_t *size)
{
    u8 *ret = NULL;
    *size = 0;
    switch (m_temp_device)
    {
    case 8:
        ret = (u8 *)sensor_mt03x_CONFIG_CMD;
        *size = 9;
        break;
    default:
        break;
    }
    //DEBUG("Send[%s]", ret);
    return ret;
}

const u8 sensor_mt03x_QUERT_CMD[12] = "QueryCalcT\r\n";
u8 *sensor_mt03x_get_query(uint32_t *size)
{
    u8 *ret = NULL;
    *size = 0;
    switch (m_temp_device)
    {
    case 8:
        ret = (u8 *)sensor_mt03x_QUERT_CMD;
        *size = 12;
        break;
    default:
        break;
    }
    //DEBUG("Send[%s]", ret);
    return ret;
}

u8 sensor_mt03x_SETA_CMD[16];
u8 *sensor_mt03x_get_setA(uint32_t *size, uint16_t cap_value)
{
    u8 *ret = NULL;
    *size = 0;
    switch (m_temp_device)
    {
    case 8:
        snprintf((char *)sensor_mt03x_SETA_CMD, 15, "SetA=1,%u\r\n", cap_value);
        ret = (u8 *)sensor_mt03x_SETA_CMD;
        *size = 13;
        break;
    default:
        break;
    }
    return ret;
}

u8 sensor_mt03x_SETE_CMD[16];
u8 *sensor_mt03x_get_setE(uint32_t *size, uint16_t value)
{
    u8 *ret = NULL;
    *size = 0;
    switch (m_temp_device)
    {
    case 8:
        snprintf((char *)sensor_mt03x_SETE_CMD, 15, "SetE=%u\r\n", value);
        ret = (u8 *)sensor_mt03x_SETE_CMD;
        *size = strlen((char *)sensor_mt03x_SETE_CMD);
        break;
    default:
        break;
    }
    return ret;
}

void sensor_mt03x_init_config(uint8_t *data, uint16_t length)
{
    //uint32_t temp = 0;
    uint16_t type = 0;
    char *oem_str = "N/A";
#if 0
    DEBUGRAW("[%u]", length);
    for (int i = 0; i < 37; ++i)
    {
        DEBUGRAW("%02X ", data[i]);
    }
    DEBUGRAW("\n");
#endif
    switch (m_temp_device)
    {
    case 8:
        if (length >= 37)
        {
            if (data[0] == 'C' && data[1] == 'N')
            {
                type = (data[11] >> 4) & 0xF;
                switch (type)
                {
                case 0:
                    oem_str = "MT";
                    break;
                case 1:
                    oem_str = "SH";
                    break;
                case 2:
                    oem_str = "DY";
                    break;
                }
                type = (data[10] + (data[11] << 8)) & 0xFFF;
                switch (type)
                {
                case 3111:
                    HOTMAP_NUM = 7;
                    m_sensor_mt03x_size_width = 32;
                    m_sensor_mt03x_size_height = 32;
                    m_sensor_mt03x_size_type = m_sensor_mt03x_size_width * m_sensor_mt03x_size_height;
                    DEBUG("Sensor: %s HPT 33 (%ux%u)", oem_str, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height);
                    break;
                case 3112:
                    HOTMAP_NUM = 8;
                    m_sensor_mt03x_size_width = 32;
                    m_sensor_mt03x_size_height = 32;
                    m_sensor_mt03x_size_type = m_sensor_mt03x_size_width * m_sensor_mt03x_size_height;
                    DEBUG("Sensor: %s HPT 90 (%ux%u)", oem_str, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height);
                    break;
                case 3120:
                    HOTMAP_NUM = 16;
                    m_sensor_mt03x_size_width = 32;
                    m_sensor_mt03x_size_height = 32;
                    m_sensor_mt03x_size_type = m_sensor_mt03x_size_width * m_sensor_mt03x_size_height;
                    DEBUG("Sensor: %s D6T-32L (%ux%u)", oem_str, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height);
                    break;
                case 3121:
                    HOTMAP_NUM = 16;
                    m_sensor_mt03x_size_width = 4;
                    m_sensor_mt03x_size_height = 4;
                    m_sensor_mt03x_size_type = m_sensor_mt03x_size_width * m_sensor_mt03x_size_height;
                    DEBUG("Sensor: %s D6T-44L (%ux%u)", oem_str, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height);
                    break;
                case 3122:
                    HOTMAP_NUM = 16;
                    m_sensor_mt03x_size_width = 1;
                    m_sensor_mt03x_size_height = 1;
                    m_sensor_mt03x_size_type = m_sensor_mt03x_size_width * m_sensor_mt03x_size_height;
                    DEBUG("Sensor: %s D6T-1A (%ux%u)", oem_str, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height);
                    break;
                case 3123:
                    m_sensor_mt03x_size_width = 8;
                    m_sensor_mt03x_size_height = 1;
                    m_sensor_mt03x_size_type = m_sensor_mt03x_size_width * m_sensor_mt03x_size_height;
                    DEBUG("Sensor: %s D6T-8L (%ux%u)", oem_str, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height);
                    break;
                }
                m_sensor_mt03x_type = type;
                m_sensor_mt03x_otype = (data[10] + (data[11] << 8));
                //app_set_device_type(8, m_sensor_mt03x_type);

                mt03x_version = (data[33 + 0] + (data[33 + 1] << 8));
                mt03x_RMode = data[25];
                mt03x_LMode = data[26];
                mt03x_epsilon = (data[29 + 0] + (data[29 + 1] << 8));
                mt03x_iir = data[31];
                mt03x_mavg = data[32];
                mt03x_id_arr[0] = data[2];
                mt03x_id_arr[1] = data[3];
                mt03x_id_arr[2] = data[4];
                mt03x_id_arr[3] = data[5];
                mt03x_id = (mt03x_id_arr[0] + (mt03x_id_arr[1] << 8) + (mt03x_id_arr[2] << 16) + (mt03x_id_arr[3] << 24));
                /*
                if (system_config.SensorId != mt03x_id)
                {
                    system_config.SensorId = mt03x_id;
                    system_config.EPSILON = 0;
                    config_sync_flash();
                }
                */
                DEBUG("Sensor Version: %04X", mt03x_version);
                DEBUG("Sensor ID: %u", mt03x_id);
                DEBUG("Sensor RMode: %u", mt03x_RMode);
                DEBUG("Sensor LMode: %u", mt03x_LMode);
                DEBUG("Sensor Epsilon: %u", mt03x_epsilon);
                DEBUG("Sensor IIR: %u", mt03x_iir);
                DEBUG("Sensor MAVG: %u", mt03x_mavg);
            }
        }
        break;
    default:
        break;
    }
    //DEBUG("========> %05.2f\n", m_sensor_mt03x_temp);
}
#if (SHOW_COLOR_OUTPUT_TYPE == COLOR_OUTPUT_TYPE_RGB565)
uint16_t colorMapBuffer[32 * 32];
#else
uint32_t colorMapBuffer[32 * 32];
#endif

static double m_step = 0;
static uint16_t m_min_temp = 0;
static uint16_t m_max_temp = 0;

void colorInit(uint16_t max_temp, uint16_t min_temp)
{
    uint16_t diff = max_temp - min_temp;
    m_step = (double)(1.0f / diff);
    m_min_temp = min_temp;
    m_max_temp = max_temp;
    m_max_temp = m_max_temp;
}
uint32_t mapJet(double v, double vmin, double vmax)
{
    uint32_t ret = 0;

    if (v < vmin)
    {
        v = vmin;
    }

    if (v > vmax)
    {
        v = vmax;
    }

    double dr, dg, db;

    if (v < 0.1242f)
    {
        db = 0.504f + ((1.f - 0.504f) / 0.1242f) * v;
        dg = dr = 0.f;
    }
    else if (v < 0.3747f)
    {
        db = 1.f;
        dr = 0.f;
        dg = (v - 0.1242f) * (1.f / (0.3747f - 0.1242f));
    }
    else if (v < 0.6253f)
    {
        db = (0.6253f - v) * (1.f / (0.6253f - 0.3747f));
        dg = 1.f;
        dr = (v - 0.3747f) * (1.f / (0.6253f - 0.3747f));
    }
    else if (v < 0.8758f)
    {
        db = 0.f;
        dr = 1.f;
        dg = (0.8758f - v) * (1.f / (0.8758f - 0.6253f));
    }
    else
    {
        db = 0.f;
        dg = 0.f;
        dr = 1.f - (v - 0.8758f) * ((1.f - 0.504f) / (1.f - 0.8758f));
    }
    ret = (((uint8_t)(255 * dr)) << 16) & 0xFF0000;
    ret = ret | ((((uint8_t)(255 * dg)) << 8) & 0xFF00);
    ret = ret | ((((uint8_t)(255 * db))) & 0xFF);

    return ret;
}

float temp_buffer_pools[32 * 32];
uint32_t getColorValue(int index, uint16_t _value)
{
    double c;
    uint16_t value;
    if (_value > 2731)
    {
        value = _value - 2731;
    }
    else
    {
        value = 0;
    }
    if (value > 0)
    {
        if (value < 3000)
        {
            c = m_step * (value - m_min_temp);
        }
        else
        {
            c = 1;
        }
    }
    else
    {
        c = 0;
    }

    return mapJet(c, 0, 1);
}

uint16_t mapRgb565(double v, double vmin, double vmax)
{
    uint16_t ret = 0;

    if (v < vmin)
    {
        v = vmin;
    }

    if (v > vmax)
    {
        v = vmax;
    }

    double dr, dg, db;

    if (v < 0.1242f)
    {
        db = 0.504f + ((1.f - 0.504f) / 0.1242f) * v;
        dg = dr = 0.f;
    }
    else if (v < 0.3747f)
    {
        db = 1.f;
        dr = 0.f;
        dg = (v - 0.1242f) * (1.f / (0.3747f - 0.1242f));
    }
    else if (v < 0.6253f)
    {
        db = (0.6253f - v) * (1.f / (0.6253f - 0.3747f));
        dg = 1.f;
        dr = (v - 0.3747f) * (1.f / (0.6253f - 0.3747f));
    }
    else if (v < 0.8758f)
    {
        db = 0.f;
        dr = 1.f;
        dg = (0.8758f - v) * (1.f / (0.8758f - 0.6253f));
    }
    else
    {
        db = 0.f;
        dg = 0.f;
        dr = 1.f - (v - 0.8758f) * ((1.f - 0.504f) / (1.f - 0.8758f));
    }

    ret = (((uint8_t)(255 * dr)) << 11) & 0xF800;
    ret = ret | ((((uint8_t)(255 * dg)) << 5) & 0x07E0);
    ret = ret | ((((uint8_t)(255 * db))) & 0x001F);

    return ret;
}

uint8_t *mrgb;
#if 0
uint16_t getRgb565ColorValue(int index, uint16_t _value)
{
    double c;
    uint16_t value;
    if (_value > 2731)
    {
        value = _value - 2731;
    }
    else
    {
        value = 0;
    }
    if (value > 0)
    {
        if (value < 3000)
        {
            c = m_step * (value - m_min_temp);
        }
        else
        {
            c = 1;
        }
    }
    else
    {
        c = 0;
    }
    //mrgb = GetColor( c, system_config.mColorMapIndex);
    mrgb = GetColor(c, Inferno);

    mrgb[0] = mrgb[0] >> 3;
    mrgb[1] = mrgb[1] >> 2;
    mrgb[2] = mrgb[2] >> 3;

    value = (uint16_t)((((uint16_t)(mrgb[0])) << 11) & 0xF800) | ((uint16_t)(((uint16_t)(mrgb[1])) << 5) & 0x07E0) | ((uint16_t)((uint16_t)(mrgb[2])) & 0x001F);

    return value;
}
#endif
volatile uint16_t mtemp_buffer[1024];
void sensor_mt03x_common_handler(uint8_t *data, uint16_t length)
{
    uint16_t max_temp, min_temp, i, j, k;
    volatile uint32_t Temp = 0;
#if 0
    DEBUGRAW("[%u]", length);
    for (int i = 0; i < 4; ++i)
    {
        DEBUGRAW("%02X ", data[i]);
    }
    DEBUGRAW("\n");
#endif
    switch (m_temp_device)
    {
    case 8:
        if (length >= sensor_mt03x_get_minisize())
        {
            if (data[0] == 'C' && data[1] == 'A')
            {
                max_temp = 0;
                min_temp = 0xFFFF;
                for (int i = 0; i < m_sensor_mt03x_size_type; ++i)
                {
                    k = 2 + (i * 2);
                    mtemp_buffer[i] = (((uint8_t)data[k + 1]) << 8) + (data[k]);
                    if (mtemp_buffer[i] >= 2731 && mtemp_buffer[i] < 5731)
                    {
                        Temp = (uint16_t)(mtemp_buffer[i] - 2731);
                        if (max_temp < Temp)
                        {
                            max_temp = Temp;
                        }
                        if (min_temp > Temp)
                        {
                            min_temp = Temp;
                        }
                    }
                }
                Temp = 0;
                if (max_temp > min_temp)
                {
                    colorInit(max_temp, min_temp);
                    for (i = 0; i < 32; ++i)
                    {
                        for (j = 0; j < 32; ++j)
                        {
                            k = j * 32 + i;
#if (SHOW_COLOR_OUTPUT_TYPE == COLOR_OUTPUT_TYPE_RGB565)
                            colorMapBuffer[k] = getRgb565ColorValue(k, mtemp_buffer[k]);
#else
                            colorMapBuffer[k] = getColorValue(k, mtemp_buffer[k]);
#endif
                            Temp = Temp + mtemp_buffer[i];
                        }
                    }
                }
                Temp = Temp / 1024;
                avgTemp = (Temp - 2731) / 10.0f;

                m_sensor_mt03x_ta = (((((uint8_t)data[m_sensor_mt03x_size_type * 2 + 2 + 1]) << 8) + (data[m_sensor_mt03x_size_type * 2 + 2])) - 2731) / 10.0;
                m_sensor_mt03x_temp = (float)(data[m_sensor_mt03x_size_type * 2 + 4] + (data[m_sensor_mt03x_size_type * 2 + 4 + 1] / 100.0));

                m_sensor_mt03x_pos_x = ((((uint8_t)data[m_sensor_mt03x_size_type * 2 + 6 + 1]) << 8) + (data[m_sensor_mt03x_size_type * 2 + 6]));
                m_sensor_mt03x_pos_y = ((((uint8_t)data[m_sensor_mt03x_size_type * 2 + 8 + 1]) << 8) + (data[m_sensor_mt03x_size_type * 2 + 8]));
                search_max_face_add(m_sensor_mt03x_temp);

                //DEBUG("[%u]===> (%05.2f)%05.2f", TimeMsSysCount - stime, min_temp / 10.0f, m_sensor_mt03x_temp);
#if 1
                if (m_sensor_mt03x_size_type > 16 || (m_sensor_mt03x_size_type >= 16 && m_big_sensor_show != 2))
                {
                    m_sensor_mt03x_flush_ui_flag = 1;

                    app_callback_flush_new_thermal_img((uint16_t *)mtemp_buffer, m_sensor_mt03x_size_width, m_sensor_mt03x_size_height, m_sensor_mt03x_temp, m_sensor_mt03x_ta);
                }
#endif
            }
        }
        break;
    default:
        break;
    }
    //printf("[%u]========> %05.2f\n", TimeSys10ms, m_sensor_mt03x_temp);
}

uint32_t sensor_mt03x_get_minisize(void)
{
    uint32_t ret = 14;
    switch (m_temp_device)
    {
    case 8:
        ret = (m_sensor_mt03x_size_type * 2) + 12;
        break;
    default:
        break;
    }
    return ret;
}

uint32_t sensor_mt03x_get_calc_count(void)
{
    uint32_t ret = 15;
    switch (m_temp_device)
    {
    case 8:
        switch (m_sensor_mt03x_type)
        {
        case 3111:
            ret = 4;
            break;
        case 3112:
            ret = 4;
            break;
        case 3120:
            ret = 4;
            break;
        case 3121:
            ret = 15;
            break;
        case 3122:
            ret = 10;
            break;
        }
        break;
    default:
        break;
    }
    return ret;
}

bool sensor_mt03x_ok(void)
{
    return (m_temp_device == 8);
}

bool sensor_mt03x_minsize_ok()
{
    if (m_big_sensor_show != 2)
    {
        return (m_temp_device == 8 && m_sensor_mt03x_size_type < 16);
    }
    else
    {
        return (m_temp_device == 8 && m_sensor_mt03x_size_type <= 16);
    }
}

bool sensor_mt03x_pos_ok()
{
    if (m_big_sensor_show != 2)
    {
        return (m_temp_device == 8 && m_sensor_mt03x_size_type >= 16);
    }
    else
    {
        return (m_temp_device == 8 && m_sensor_mt03x_size_type > 16);
    }
}

void search_max_face_add(float face_temp)
{
    hotMap[hotIndex] = face_temp;
    hotIndex++;
    if (hotIndex >= HOTMAP_MAX)
    {
        hotIndex = 0;
    }
}

float search_max_face_temp(void)
{
    float max_temp_new = 0.0 /*, max_temp_total = 0*/;
    volatile uint8_t m_hotIndex = 0, x_hotIndex = 0;
    //float lastTemp = 0.f; // ×îºóÒ»´ÎÎÂ¶ÈÖµ
    //float alpha = 0.05f;   // Ê±Óò¸üÐÂÈ¨ÖØ
    //double maxT;
    m_hotIndex = hotIndex;
    if (hotIndex < HOTMAP_NUM)
    {
        if (hotIndex != 0)
        {
            for (int i = 0; i < (HOTMAP_NUM - hotIndex); ++i)
            {
                hotCalMap[i] = hotMap[(HOTMAP_MAX - (HOTMAP_NUM - hotIndex)) + i];
            }
            for (int i = 0; i < hotIndex; ++i)
            {
                hotCalMap[HOTMAP_NUM - hotIndex + i] = hotMap[i];
            }
        }
        else
        {
            for (int i = 0; i < HOTMAP_NUM; ++i)
            {
                hotCalMap[i] = hotMap[(HOTMAP_MAX - HOTMAP_NUM) + i];
            }
        }
    }
    else
    {
        for (int i = 0; i < HOTMAP_NUM; ++i)
        {
            hotCalMap[i] = hotMap[(hotIndex - HOTMAP_NUM) + i];
        }
    }
    for (int i = 0; i < HOTMAP_NUM; ++i)
    {
        if (max_temp_new < hotCalMap[i])
        {
            max_temp_new = hotCalMap[i];
            x_hotIndex = i;
        }
    }
#if 0
    if (m_temp_blur_mode == 3)
    {
        lastTemp = hotCalMap[0];
        for (int i = 0; i < HOTMAP_NUM; ++i)
        {
            maxT = alpha * hotCalMap[i] + (1 - alpha) * lastTemp;
            lastTemp = hotCalMap[i];
        }
        max_temp_new = lastTemp;
    }
    else
    {
#else
    //printf("|CC> %u,%u,%05.2f\n", m_hotIndex, x_hotIndex, max_temp_new);
    if (m_sensor_mt03x_type != 3112)
    {
#if 0
        if (HOTMAP_NUM > 6)
        {
            if (x_hotIndex < (HOTMAP_NUM - 5))
            { // May be away
                for (int i = 0; i < 5; ++i)
                {
                    max_temp_total = hotCalMap[i + x_hotIndex] + max_temp_total;
                    //printf(" %05.2f", hotCalMap[i + x_hotIndex]);
                }
                max_temp_new = max_temp_total / 5.0f;
            }
            else
            { // May be approaching
                // 5 - > 6, More stable from the data
                for (int i = 0; i < 6; ++i)
                {
                    max_temp_total = hotCalMap[i + (HOTMAP_NUM - 6)] + max_temp_total;
                    //printf(" %05.2f", hotCalMap[i + (HOTMAP_NUM - 5)]);
                }
                max_temp_new = max_temp_total / 6.0f;
            }
        }
#endif
    }
    else
    {
#if 0
        for (int i = 0; i < HOTMAP_NUM; ++i)
        {
            max_temp_total = hotCalMap[i] + max_temp_total;
            //printf(" %05.2f", hotCalMap[i + x_hotIndex]);
        }
        printf("|CC> %05.2f -> %05.2f\n", max_temp_new, max_temp_total / (float)HOTMAP_NUM);
        max_temp_new = max_temp_total / (float)HOTMAP_NUM;
#endif
    }
#endif
    //printf("\n");
    return max_temp_new;
}

float sensor_mt03x_get_temp(void)
{
    return search_max_face_temp();
}

float sensor_mt03x_get_avg_temp(void)
{
    return avgTemp;
}

float sensor_mt03x_get_cur_temp(void)
{
    return m_sensor_mt03x_temp;
}

uint8_t sensor_is_90d_type(void)
{
    if (m_sensor_mt03x_type == 3112)
        return 1;
    else
        return 0;
}
